【精选】ROS下订阅消息并对消息进行处理 |
您所在的位置：网站首页 › ros 订阅 发布 › 【精选】ROS下订阅消息并对消息进行处理 |
【精选】ROS下订阅消息并对消息进行处理
|
ROS下订阅自定义的消息并对消息进行处理
前言ROS自定义msgpublisher发布subscriber订阅对订阅的消息进行处理CMakeLists编译结果展示
前言
站内已经有很多博客文章教程帮助实现订阅ROS消息并直接输出到terminal下。本文研究如何进一步对订阅的消息进行简单的处理。本文的例子实现了以下过程： 1.自定义了四个参数{c0, c1, c2, c3}并通过publisher发布到ros节点中； 2.订阅publisher里面的消息，将其作为曲线三次方程的系数，在rviz下画出这条线。 以下是实现的过程： ROS自定义msg参考这篇博文 新建工作空间linemessage_ws/src在src目录下初始化 catkin_init_workspace 在src目录下建立pkg catkin_create_pkg linemessage roscpp rospy visualization_msgs std_msgs 返回工作空间linemessage_ws并运行 catkin_make 进入linemessage_ws/src下的linemessage文件夹并在此目录下新建msg文件夹在msg文件夹下新建data.msg文件，在data.msg中自定义自己的消息 float32 c0 float32 c1 float32 c2 float32 c3 保存后在msg文件夹同级目录中编辑CMakeLists.txt文件，主要编辑内容为： 在find_package中添加message_generation 在find_package后添加 add_message_files(FILES data.msg) generate_messages(DEPENDENCIES std_msgs)在catkin_package中添加 CATKIN_DEPENDS roscpp rospy std_msgs visualization_msgs message_runtime 编辑package.xml文件 linemessage 0.0.0 The linemessage package saic TODO catkin roscpp rospy std_msgs visualization_msgs roscpp rospy std_msgs visualization_msgs roscpp rospy std_msgs visualization_msgs message_generation message_runtime 保存后回到工作空间linemessage_ws并运行 catkin_make publisher发布参考这篇博文 在linemessage_ws/src/linemessage/src目录下新建publisher.cpp文件，输入代码如下 #include #include "linemessage/data.h" int main(int argc, char **argv) { ros::init(argc, argv, "data_publisher"); ros::NodeHandle n; ros::Publisher data_info_pub = n.advertise("/data_info", 10); ros::Rate loop_rate(1); int count = 0; while (ros::ok()) { linemessage::data data_msg; data_msg.c0 = 2.5f; data_msg.c1 = 0.5f; data_msg.c2 = 0.2f; data_msg.c3 = 0.1f; data_info_pub.publish(data_msg); ROS_INFO("Publish data Info: c0:%f c1:%f c2:%f c3:%f", data_msg.c0, data_msg.c1, data_msg.c2, data_msg.c3); loop_rate.sleep(); } return 0; }输入完并保存。 subscriber订阅在linemessage_ws/src/linemessage/src目录下新建subscriber.cpp文件，输入代码如下 #include #include "linemessage/data.h" void dataInfoCallback(const linemessage::data::ConstPtr& msg) { ROS_INFO("Publish data Info: c0:%f c1:%f c2:%f c3:%f", msg->c0, msg->c1, msg->c2, msg->c3); } int main(int argc, char **argv) { ros::init(argc, argv, "data_subscriber"); ros::NodeHandle n; ros::Subscriber data_info_sub = n.subscribe("/data_info", 10, dataInfoCallback); ros::spin(); return 0; }输入完并保存。 对订阅的消息进行处理参考ROS官方教程 参考这篇博文 与subscriber.cpp类似的订阅方法，实现订阅publisher里面的消息，将其作为曲线三次方程的系数，在rviz下画出这条线。在linemessage_ws/src/linemessage/src目录下新建points_and_lines.cpp文件，输入代码如下 #include #include #include "linemessage/data.h" #include class points_and_lines { private: ros::NodeHandle n; ros::Subscriber data_info_sub; ros::Publisher marker_pub; public: points_and_lines() { data_info_sub = n.subscribe("/data_info", 10, &points_and_lines::dataInfoCallback, this); marker_pub = n.advertise("visualization_marker", 10); } void dataInfoCallback(const linemessage::data::ConstPtr& msg); }; void points_and_lines::dataInfoCallback(const linemessage::data::ConstPtr& msg) { ROS_INFO("Publish data Info: c0:%f c1:%f c2:%f c3:%f", msg->c0, msg->c1, msg->c2, msg->c3); float c0 = msg->c0; float c1 = msg->c1; float c2 = msg->c2; float c3 = msg->c3; ros::Rate r(30); float f = 0.0; visualization_msgs::Marker points, line_strip, line_list; points.header.frame_id = line_strip.header.frame_id = line_list.header.frame_id = "map"; points.header.stamp = line_strip.header.stamp = line_list.header.stamp = ros::Time::now(); points.ns = line_strip.ns = line_list.ns = "points_and_lines"; points.action = line_strip.action = line_list.action = visualization_msgs::Marker::ADD; points.pose.orientation.w = line_strip.pose.orientation.w = line_list.pose.orientation.w = 1.0; points.id = 0; line_strip.id = 1; line_list.id = 2; points.type = visualization_msgs::Marker::POINTS; line_strip.type = visualization_msgs::Marker::LINE_STRIP; line_list.type = visualization_msgs::Marker::LINE_LIST; // POINTS markers use x and y scale for width/height respectively points.scale.x = 0.2; points.scale.y = 0.2; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip.scale.x = 0.1; line_list.scale.x = 0.1; // Points are green points.color.g = 1.0f; points.color.a = 0.0; // Line strip is blue line_strip.color.b = 1.0; line_strip.color.a = 1.0; // Line list is red line_list.color.r = 1.0; line_list.color.a = 0.0; // Create the vertices for the points and lines for (uint32_t i = 0; i ros::init(argc, argv, "points_and_lines"); //ros::Subscriber data_info_sub = n.subscribe("/data_info", 10, dataInfoCallback); //ros::Publisher marker_pub = n.advertise("visualization_marker", 10); //ros::Rate r(30); //float f = 0.0; /* float c0 = 1.0; float c1 = 1.1; float c2 = 1.2; float c3 = 1.3;*/ points_and_lines pal; ros::spin(); return 0; }输入完并保存。 CMakeLists编译编译使用CMakeLists比较简单，在linemessage_ws/src/linemessage目录下的CMakeLists.txt末尾添加下列代码： add_executable(data_publisher src/publisher.cpp) target_link_libraries(data_publisher ${catkin_LIBRARIES}) add_dependencies(data_publisher ${PROJECT_NAME}_generate_messages_cpp) add_executable(data_subscriber src/subscriber.cpp) target_link_libraries(data_subscriber ${catkin_LIBRARIES}) add_dependencies(data_subscriber ${PROJECT_NAME}_generate_messages_cpp) add_executable(points_and_lines src/points_and_lines.cpp) target_link_libraries(points_and_lines ${catkin_LIBRARIES}) add_dependencies(points_and_lines ${PROJECT_NAME}_generate_messages_cpp)整个CMakeLists.txt的代码为 cmake_minimum_required(VERSION 3.0.2) project(linemessage) ## Compile as C++11, supported in ROS Kinetic and newer # add_compile_options(-std=c++11) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs visualization_msgs message_generation ) ## System dependencies are found with CMake's conventions # find_package(Boost REQUIRED COMPONENTS system) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html # catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## ################################################ ## To declare and build messages, services or actions from within this ## package, follow these steps: ## * Let MSG_DEP_SET be the set of packages whose message types you use in ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). ## * In the file package.xml: ## * add a build_depend tag for "message_generation" ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in ## but can be declared for certainty nonetheless: ## * add a exec_depend tag for "message_runtime" ## * In this file (CMakeLists.txt): ## * add "message_generation" and every package in MSG_DEP_SET to ## find_package(catkin REQUIRED COMPONENTS ...) ## * add "message_runtime" and every package in MSG_DEP_SET to ## catkin_package(CATKIN_DEPENDS ...) ## * uncomment the add_*_files sections below as needed ## and list every .msg/.srv/.action file to be processed ## * uncomment the generate_messages entry below ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) ## Generate messages in the 'msg' folder # add_message_files( # FILES # Message1.msg # Message2.msg # ) ## Generate services in the 'srv' folder # add_service_files( # FILES # Service1.srv # Service2.srv # ) ## Generate actions in the 'action' folder # add_action_files( # FILES # Action1.action # Action2.action # ) ## Generate added messages and services with any dependencies listed here # generate_messages( # DEPENDENCIES # std_msgs# visualization_msgs # ) add_message_files(FILES data.msg) generate_messages(DEPENDENCIES std_msgs) ################################################ ## Declare ROS dynamic reconfigure parameters ## ################################################ ## To declare and build dynamic reconfigure parameters within this ## package, follow these steps: ## * In the file package.xml: ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" ## * In this file (CMakeLists.txt): ## * add "dynamic_reconfigure" to ## find_package(catkin REQUIRED COMPONENTS ...) ## * uncomment the "generate_dynamic_reconfigure_options" section below ## and list every .cfg file to be processed ## Generate dynamic reconfigure parameters in the 'cfg' folder # generate_dynamic_reconfigure_options( # cfg/DynReconf1.cfg # cfg/DynReconf2.cfg # ) ################################### ## catkin specific configuration ## ################################### ## The catkin_package macro generates cmake config files for your package ## Declare things to be passed to dependent projects ## INCLUDE_DIRS: uncomment this if your package contains header files ## LIBRARIES: libraries you create in this project that dependent projects also need ## CATKIN_DEPENDS: catkin_packages dependent projects also need ## DEPENDS: system dependencies of this project that dependent projects also need catkin_package( # INCLUDE_DIRS include # LIBRARIES linemessage CATKIN_DEPENDS roscpp rospy std_msgs visualization_msgs message_runtime # DEPENDS system_lib ) ########### ## Build ## ########### ## Specify additional locations of header files ## Your package locations should be listed before other locations include_directories( # include ${catkin_INCLUDE_DIRS} ) ## Declare a C++ library # add_library(${PROJECT_NAME} # src/${PROJECT_NAME}/linemessage.cpp # ) ## Add cmake target dependencies of the library ## as an example, code may need to be generated before libraries ## either from message generation or dynamic reconfigure # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Declare a C++ executable ## With catkin_make all packages are built within a single CMake context ## The recommended prefix ensures that target names across packages don't collide # add_executable(${PROJECT_NAME}_node src/linemessage_node.cpp) ## Rename C++ executable without prefix ## The above recommended prefix causes long target names, the following renames the ## target back to the shorter version for ease of user use ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") ## Add cmake target dependencies of the executable ## same as for the library above # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) ## Specify libraries to link a library or executable target against # target_link_libraries(${PROJECT_NAME}_node # ${catkin_LIBRARIES} # ) ############# ## Install ## ############# # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination # catkin_install_python(PROGRAMS # scripts/my_python_script # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark executables for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html # install(TARGETS ${PROJECT_NAME}_node # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} # ) ## Mark libraries for installation ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html # install(TARGETS ${PROJECT_NAME} # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} # ) ## Mark cpp header files for installation # install(DIRECTORY include/${PROJECT_NAME}/ # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} # FILES_MATCHING PATTERN "*.h" # PATTERN ".svn" EXCLUDE # ) ## Mark other files for installation (e.g. launch and bag files, etc.) # install(FILES # # myfile1 # # myfile2 # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} # ) ############# ## Testing ## ############# ## Add gtest based cpp test target and link libraries # catkin_add_gtest(${PROJECT_NAME}-test test/test_linemessage.cpp) # if(TARGET ${PROJECT_NAME}-test) # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) # endif() ## Add folders to be run by python nosetests # catkin_add_nosetests(test) add_executable(data_publisher src/publisher.cpp) target_link_libraries(data_publisher ${catkin_LIBRARIES}) add_dependencies(data_publisher ${PROJECT_NAME}_generate_messages_cpp) add_executable(data_subscriber src/subscriber.cpp) target_link_libraries(data_subscriber ${catkin_LIBRARIES}) add_dependencies(data_subscriber ${PROJECT_NAME}_generate_messages_cpp) add_executable(points_and_lines src/points_and_lines.cpp) target_link_libraries(points_and_lines ${catkin_LIBRARIES}) add_dependencies(points_and_lines ${PROJECT_NAME}_generate_messages_cpp) 结果展示返回工作空间linemessage_ws目录下，运行 catkin_make编译完成后运行 roscore打开一个新的terminal，输入 source devel/setup.sh然后输入 rosrun linemessage data_publisher效果为 打开新的terminal，输入 source devel/setup.sh然后输入 rosrun linemessage data_subscriber效果为 打开新的terminal，输入 source devel/setup.sh然后输入 rosrun linemessage points_and_lines效果为 打开新的terminal，输入 rviz打开rviz，点击add，在By topic选项卡下选中/visualization_maker下的Marker，点击OK，可在rviz界面下看到如下效果 至此，生成完毕。 相关程序已经上传到CSDN下载 下载界面 |
【本文地址】